import RPi.GPIO as GPIO
import time
import threading
import cv2

# 导入新的电机控制模块
from motor_controller import init_gpio as init_gpio_motor
from motor_controller import PID, start_speed_monitor, start_pwm_update_daemon, \
    set_motor_speed, rotate_in_place, drive_straight, drive_with_color, \
    stop_motor, cleanup as cleanup_motor

# 导入颜色检测模块
from detect_color import init_camera, start_color_detection, \
    get_latest_color_data, cleanup as cleanup_camera

# 导入超声波模块
from detect_distance import init_i2c, measure_distance, \
    start_distance_measurement, get_latest_distance, \
    cleanup as cleanup_distance


# 状态管理类
class StateManager:
    def __init__(self):
        self.current_state = 1  # 当前状态
        self.colors = {}        # 存储识别到的魔方颜色
        self.state_1_done = False  # 状态1是否完成
        self.state_2_done = False  # 状态2是否完成
        self.lost_target_time = None  # 丢失目标的时间
        self.on_target_1_time = None  # 接近目标的时间
        self.on_target_2_time = None  # 状态2接近目标的时间

    def set_state(self, new_state):
        """设置当前状态"""
        self.current_state = new_state
        print(f"状态已更改为: {new_state}")

    def reset_timers(self):
        """重置所有计时器"""
        self.lost_target_time = None
        self.on_target_1_time = None
        self.on_target_2_time = None


# 全局变量
running = True
state_manager = StateManager()  # 创建状态管理器实例
display_camera = True  # 是否显示摄像头画面
last_seen_time = None  # 上次看到魔方的时间

# 颜色识别相关参数
COLOR_CONFIRM_COUNT = 3  # 需要连续识别相同颜色的次数
color_confirm_counter = {}  # 颜色确认计数器
confirmed_color = None  # 已确认的颜色

# 避障和导航参数
MIN_DISTANCE = 20  # 最小安全距离(cm)
BEGIN_DISTANCE = 30  # 开始避障距离(cm)
ON_TAGERT_TIME = 2  # 靠近目标后的绕目标旋转时间(秒)
SIDE_OFFSET = 50   # 侧向偏移像素值(用于绕过魔方)
STRAIGHT_TIME = 2  # 通过魔方后直行的时间(秒)

# 左转和右转的颜色分类
LEFT_COLORS = ["red", "yellow"]
RIGHT_COLORS = ["blue", "green"]

# 显示摄像头画面的线程函数
def display_camera_thread():
    """
    显示摄像头实时画面的线程函数
    """
    global running, display_camera
    
    if not 'camera' in globals() or camera is None:
        print("错误: 摄像头未初始化，无法显示画面")
        return
    
    print("摄像头显示线程已启动")
    
    # 创建窗口
    cv2.namedWindow("摄像头画面", cv2.WINDOW_NORMAL)
    cv2.resizeWindow("摄像头画面", 640, 480)
    
    while running and display_camera:
        # 读取一帧图像
        ret, frame = camera.read()
        if not ret:
            print("警告: 无法从摄像头读取图像")
            time.sleep(0.1)
            continue
        
        # 获取最新的颜色检测结果，在画面上标记出来
        color_data = get_latest_color_data()
        
        # 在画面上标记检测到的颜色区域
        for color, segments in color_data.items():
            for segment in segments:
                x_start, x_end, x_center = segment
                # 计算y坐标（根据detect_color.py中的DEFAULT_ROW_PERCENT和DEFAULT_ROW_HEIGHT）
                height = frame.shape[0]
                row_percent = 0.5  # 默认值，与detect_color.py中保持一致
                row_height = 10    # 默认值，与detect_color.py中保持一致
                y_center = int(height * row_percent)
                
                # 在图像上绘制矩形标记颜色区域
                color_rgb = (0, 0, 255)  # 默认红色
                if color == "red":
                    color_rgb = (0, 0, 255)
                elif color == "green":
                    color_rgb = (0, 255, 0)
                elif color == "blue":
                    color_rgb = (255, 0, 0)
                elif color == "yellow":
                    color_rgb = (0, 255, 255)
                
                cv2.rectangle(frame, (x_start, y_center-row_height//2), 
                             (x_end, y_center+row_height//2), color_rgb, 2)
                cv2.putText(frame, color, (x_start, y_center-15), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_rgb, 2)
        
        # 显示距离信息
        distance = get_latest_distance()
        cv2.putText(frame, f"距离: {distance:.1f} cm", (10, 30), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        
        # 显示当前状态
        cv2.putText(frame, f"状态: {state_manager.current_state}", (10, 60), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        
        # 显示图像
        cv2.imshow("摄像头画面", frame)
        
        # 按下q键退出
        if cv2.waitKey(1) & 0xFF == ord('q'):
            display_camera = False
            break
        
        # 控制帧率
        time.sleep(0.03)  # 约30fps
    
    # 关闭窗口
    cv2.destroyAllWindows()
    print("摄像头显示线程已停止")

# 启动显示摄像头画面的线程
def start_display_camera():
    """启动显示摄像头画面的线程"""
    global display_thread
    
    display_thread = threading.Thread(target=display_camera_thread)
    display_thread.daemon = True  # 设为守护线程，主程序结束时自动结束
    display_thread.start()
    
    return display_thread

# 基于颜色检测的直线行驶函数（适配新的电机控制模块）
def drive_straight_with_color(color_offset, speed=1.5, offset_factor=0.2):
    """
    根据颜色检测的偏移量控制小车行驶
    
    Args:
        color_offset: 颜色中心相对于画面中心的偏移量
        speed: 基础速度（转/秒）
        offset_factor: 偏移影响因子，范围0到1
    """
    # 使用新模块的drive_with_color函数
    drive_with_color(color_offset, speed, offset_factor)

# 使用PID控制的原地转向函数（适配新的电机控制模块）
def rotate_in_place_with_pid(direction, target_speed=1.0):
    """
    控制小车原地转向
    
    Args:
        direction: 转向方向，'clockwise'顺时针，'counterclockwise'逆时针
        target_speed: 转向速度（转/秒）
    """
    # 使用新模块的rotate_in_place函数
    rotate_in_place(direction, target_speed)

# 使用PID控制的直线行驶函数（适配新的电机控制模块）
def drive_straight_with_pid(target_speed=1.5):
    """
    控制小车直线行驶
    
    Args:
        target_speed: 目标速度，正值表示前进，负值表示后退
    """
    # 使用新模块的drive_straight函数
    drive_straight(target_speed)


try:
    # 初始化电机
    pwma, pwmb = init_gpio_motor()
    print("电机已初始化")
    
    # 启动电机速度监测
    start_speed_monitor()
    print("电机速度监测已启动")
    
    # 启动PWM更新守护进程
    start_pwm_update_daemon()
    print("PWM更新守护进程已启动")

    # 启动超声波模块
    init_i2c()
    start_distance_measurement()  # 启动超声波模块
    print("超声波模块已启动")
    
    # 初始化摄像头
    camera = init_camera() # 摄像头ID默认为0，可选输入摄像头ID
    if camera is None:
        print("摄像头初始化失败，程序退出")
        exit(1)
    
    # 启动颜色检测线程
    start_color_detection(interval=0.1)  # 每0.1秒检测一次
    print("颜色检测线程已启动")
    
    # 启动显示摄像头画面的线程
    start_display_camera()
    print("摄像头画面显示已启动")

    # 等待系统稳定
    print("系统初始化中，请稍候...")
    time.sleep(2)
    
    # 主控制循环
    while running:
        # 获取最新的颜色检测结果
        color_data = get_latest_color_data()

        # 获取超声波距离
        distance = get_latest_distance()
        
        # 状态1：识别并通过第一个魔方
        if state_manager.current_state == 1:
            print("当前状态: 1 - 识别并通过第一个魔方")
            
            # 如果没有检测到颜色，直行寻找魔方
            if not color_data and not confirmed_color:
                print("未检测到魔方，直行寻找...")
                drive_straight_with_pid(target_speed=1.5)
                time.sleep(0.1)
                continue
            
            # 如果已经确认了颜色，直接使用该颜色进行导航
            if confirmed_color:
                segments = color_data.get(confirmed_color, [])
                if segments:
                    # 先前漏识别了魔方，删除计时器
                    state_manager.lost_target_time = None
                    # 记录最后一次看到魔方的时间
                    last_seen_time = time.time()
                    
                    segment = segments[0]  # 取第一个段
                    x_start, x_end, x_center = segment
                    
                    # 根据距离判断行为
                    if distance < MIN_DISTANCE and abs(x_center) < 50:
                        # 距离太近，判断是否会碰撞
                        # 如果魔方在中间，可能会碰撞
                        print("魔方距离太近且在中间，倒车避让")
                        drive_straight_with_pid(target_speed=-1.0)
                        
                    else:
                        # 距离恰当，根据颜色选择绕行方向
                        distance < BEGIN_DISTANCE #and abs(x_center) < 70
                        target_offset = 0
                        
                        if confirmed_color in LEFT_COLORS:
                            # 左侧绕行，瞄准左边缘左侧
                            target_offset = x_start - SIDE_OFFSET
                            print(f"左侧绕行，目标偏移: {target_offset}")
                        elif confirmed_color in RIGHT_COLORS:
                            # 右侧绕行，瞄准右边缘右侧
                            target_offset = x_end + SIDE_OFFSET
                            print(f"右侧绕行，目标偏移: {target_offset}")
                        
                        # 使用颜色偏移控制小车
                        drive_straight_with_color(target_offset, speed=1.5)
                        
                else:
                    if distance < MIN_DISTANCE:
                        # 距离太近，判断是否会碰撞
                        # 如果魔方在中间，可能会碰撞
                        print("魔方距离太近且在中间，倒车避让")
                        drive_straight_with_pid(target_speed=-1.0)
                        time.sleep(0.1)
                        continue
                    
                    # 已确认颜色但找不到该颜色的魔方
                    # 可能是已经绕过了魔方，开始计时
                    current_time = time.time()
                    
                    # 如果之前看到过魔方，并且已经过了一定时间没看到
                    if last_seen_time is not None and (current_time - last_seen_time) > 0.5:
                        if state_manager.lost_target_time is None:
                            state_manager.lost_target_time = current_time
                            print(f"找不到已确认的{confirmed_color}魔方，开始计时...")
                            drive_straight_with_pid(target_speed=1.0)
                        else:
                            # 检查是否已经持续一段时间没有检测到目标魔方
                            if current_time - state_manager.lost_target_time > 1.5:  # 假设1.5秒没检测到就是通过了
                                print("持续一段时间未检测到魔方，认为已通过，继续直行一段时间")
                                # 重置计时器
                                state_manager.lost_target_time = None
                                last_seen_time = None  # 重置最后看到时间
                                
                                # 通过后直行一段时间
                                start_time = time.time()
                                while time.time() - start_time < STRAIGHT_TIME:
                                    drive_straight_with_pid(target_speed=1.5)
                                    time.sleep(0.1)
                            
                                # 完成状态1，进入状态2
                                state_manager.state_1_done = True
                                state_manager.set_state(2)
                                confirmed_color = None  # 重置确认的颜色
                                color_confirm_counter = {}  # 重置颜色计数器
                                print("进入状态2: 寻找两个同色魔方")
                        
                    else:
                        # 刚刚丢失魔方，继续直行一小段时间
                        print("刚刚丢失魔方目标，继续直行...")
                        drive_straight_with_pid(target_speed=1.0)
            else:
                # 还没有确认颜色，需要进行颜色确认
                # 找到最宽的颜色块作为目标魔方
                widest_color = None
                max_width = 0
                
                for color, segments in color_data.items():
                    if segments:  # 确保有检测到该颜色
                        for segment in segments:  
                            width = abs(segment[1] - segment[0])  # 计算宽度
                            if width > max_width:
                                max_width = width
                                widest_color = color
                
                if widest_color:
                    # 颜色确认逻辑
                    if widest_color not in color_confirm_counter:
                        color_confirm_counter[widest_color] = 1
                    else:
                        color_confirm_counter[widest_color] += 1
                    
                    # 重置其他颜色的计数
                    for color in color_confirm_counter:
                        if color != widest_color:
                            color_confirm_counter[color] = 0
                    
                    # 检查是否达到确认阈值
                    if color_confirm_counter.get(widest_color, 0) >= COLOR_CONFIRM_COUNT:
                        confirmed_color = widest_color
                        state_manager.colors[1] = confirmed_color  # 记录状态1的颜色
                        print(f"已确认魔方颜色: {confirmed_color}")
                    else:
                        print(f"正在确认颜色: {widest_color} ({color_confirm_counter.get(widest_color, 0)}/{COLOR_CONFIRM_COUNT})")
                        # 在确认过程中，慢速直行
                        drive_straight_with_pid(target_speed=1.0)
                else:
                    # 没有检测到任何颜色，直行寻找
                    print("未检测到任何颜色，直行寻找...")
                    drive_straight_with_pid(target_speed=1.5)
        
        # 状态2：寻找两个同色魔方
        elif state_manager.current_state == 2:
            print("当前状态: 2 - 寻找两个同色魔方")
            
            # 原地旋转寻找魔方
            if not color_data:
                print("未检测到魔方，原地旋转寻找...")
                rotate_in_place_with_pid('clockwise', target_speed=1.0)
                time.sleep(0.1)
                continue
            
            # 寻找有两个或以上段的颜色
            target_color = None
            for color, segments in color_data.items():
                print("colors: ", color, segments)
                if len(segments) >= 2:
                    target_color = color
                    break
            
            if target_color:
                # 找到了两个同色魔方
                if confirmed_color != target_color:
                    # 重置确认计数
                    confirmed_color = None
                    color_confirm_counter = {}
                    
                    # 颜色确认逻辑
                    if target_color not in color_confirm_counter:
                        color_confirm_counter[target_color] = 1
                    else:
                        color_confirm_counter[target_color] += 1
                    
                    # 检查是否达到确认阈值
                    if color_confirm_counter.get(target_color, 0) >= COLOR_CONFIRM_COUNT:
                        confirmed_color = target_color
                        state_manager.colors[2] = confirmed_color  # 记录状态2的颜色
                        print(f"已确认两个同色魔方颜色: {confirmed_color}")
                    else:
                        print(f"正在确认两个同色魔方颜色: {target_color} ({color_confirm_counter.get(target_color, 0)}/{COLOR_CONFIRM_COUNT})")
                        # 在确认过程中，慢速旋转
                        rotate_in_place_with_pid('clockwise', target_speed=0.5)
                else:
                    # 已确认颜色，开始导航
                    segments = color_data[confirmed_color]
                    if len(segments) >= 2:
                        # 计算两个魔方之间的中点
                        segment1 = segments[0]
                        segment2 = segments[1]
                        x1_start, x1_end, x1_center = segment1
                        x2_start, x2_end, x2_center = segment2
                        
                        # 计算中点
                        target_offset = (x1_center + x2_center) // 2
                        
                        # 根据距离判断行为
                        if distance < MIN_DISTANCE and abs(target_offset) < 50:
                            # 距离太近，判断是否会碰撞
                            print("魔方距离太近且在中间，倒车避让")
                            drive_straight_with_pid(target_speed=-1.0)
                        else:
                            # 距离恰当，瞄准两个魔方之间的中点
                            print(f"瞄准两个魔方之间的中点，目标偏移: {target_offset}")
                            drive_straight_with_color(target_offset, speed=1.5)
                    else:
                        # 已确认颜色但找不到两个魔方
                        # 可能是已经通过了魔方，开始计时
                        if state_manager.lost_target_time is None:
                            state_manager.lost_target_time = time.time()
                            print(f"找不到两个{confirmed_color}魔方，开始计时...")
                            drive_straight_with_pid(target_speed=1.0)
                        else:
                            # 检查是否已经持续一段时间没有检测到目标魔方
                            if time.time() - state_manager.lost_target_time > 1.5:
                                print("持续一段时间未检测到两个魔方，认为已通过，继续直行一段时间")
                                # 重置计时器
                                state_manager.lost_target_time = None
                                
                                # 通过后直行一段时间
                                start_time = time.time()
                                while time.time() - start_time < STRAIGHT_TIME:
                                    drive_straight_with_pid(target_speed=1.5)
                                    time.sleep(0.1)
                                
                                # 完成状态2，进入状态3
                                state_manager.state_2_done = True
                                state_manager.set_state(3)
                                confirmed_color = None  # 重置确认的颜色
                                color_confirm_counter = {}  # 重置颜色计数器
                                print("进入状态3: 任务完成")
                            else:
                                # 还在计时中，继续直行
                                print(f"找不到两个{confirmed_color}魔方，继续寻找... ({time.time() - state_manager.lost_target_time:.1f}秒)")
                                drive_straight_with_pid(target_speed=1.0)
            else:
                # 没有找到两个同色魔方，继续旋转寻找
                print("未找到两个同色魔方，继续旋转寻找...")
                rotate_in_place_with_pid('clockwise', target_speed=0.8)
        
        # 状态3：任务完成
        elif state_manager.current_state == 3:
            print("当前状态: 3 - 任务完成")
            print("已完成所有任务！")
            
            # 停止电机
            stop_motor()
            
            # 显示结果
            print("识别到的魔方颜色:")
            for state_num, color in state_manager.colors.items():
                print(f"状态{state_num}: {color}")
            
            # 退出主循环
            running = False
        
        # 短暂延时，避免CPU占用过高
        time.sleep(0.1)
        
except KeyboardInterrupt:
    print("\n程序被用户中断")
finally:
    # 停止电机
    stop_motor()
    
    # 清理资源
    cleanup_motor()
    cleanup_camera()
    cleanup_distance()
    
    print("程序已退出，资源已清理")